"""  
    需求：提交两个整形数据，并处理响应结果
    流程：
        前提：main函数中需要判断提交的参数是否正确。
        1.导包；
        2.初始化ROS2客户端；
        3.自定义节点类；
            3-1.创建客户端；
            3-2.等待服务连接；
            3-3.组织请求数据并发送；            
        4.调用spain函数，并传入节点对象；
            需要调用连接服务的函数，根据连接结果做下一步处理；
            连接服务后
            再处理响应结果
        5.资源释放。


"""
# 1.导包；
import sys
import rclpy
from rclpy.node import Node
from ardupilot_msgs.srv import ModeSwitch #这是一个自定义的 ROS 2 服务消息类型（在 ardupilot_msgs 包中定义），用于启动或停止电机。该服务的请求消息包含一个 arm 字段，通常是一个布尔值，指示是否启动电机。
#1.现在需要修改mode服务
from std_srvs.srv import SetBool #这是 ROS 中标准的服务类型之一，通常用于启用或禁用某些功能。
import time #用于控制重试之间的时间间隔（例如，等待 5 秒后重试）。

# 自定义节点类；
class ModeSwitchClient(Node):#6.
    def __init__(self):
        super().__init__('mode_switch_client')#2.传入节点名称
        self.get_logger().info("客户端mode已创建！(python)")
        
        self.client = self.create_client(ModeSwitch, '/ap/mode_switch')#3.(服务接口类型，话题名称)
        while not self.client.wait_for_service(timeout_sec=3.0):
            self.get_logger().info('Mode Service not available, waiting again...')
            time.sleep(10)  # 给服务一些时间准备

    def send_request(self):
        request = ModeSwitch.Request() #4.
        request.mode = 15              #5.
        return self.client.call_async(request)

def main():
    # 2.初始化ROS2客户端；
    rclpy.init()
    mode = ModeSwitchClient()

    # 进行重试，直到服务成功
    success = False
    while not success:
        future = mode.send_request()
        rclpy.spin_until_future_complete(mode, future)

        if future.result() is not None and future.result().status:#7.
            mode.get_logger().info('Mode switch successful!')
            success = True
        else:
            mode.get_logger().warn('Mode switch failed, retrying in 5 seconds...')
            time.sleep(10)  # 等待 5 秒后重试

    mode.get_logger().info('mode 15 successfully.')
    # 4.调用spain函数，并传入节点对象；
    # rclpy.spin(MyNode())
    # client = ArmMotorsClient()
    # 5.资源释放。
    mode.destroy_node()#8.
    rclpy.shutdown()

if __name__ == '__main__':
    main()